/*
 * fetch.c
 *
 *  Created on: May 21, 2021
 *      Author: kirk_
 */

#include "fetch.h"

volatile uint32_t exti_times = 0;

void FetchThread(void *argument)
{
	int i = 0;
	uint8_t res = 1;
	float g;

	/* Init HC-SR04 */
	HCSR04_Init(&HCSR04, TRIG1_GPIO_Port, TRIG1_Pin, ECHO1_GPIO_Port, ECHO1_Pin);

	/* Init mpu6050 */
	while(res)
	{
		while(!MPU6050_Init(&MPU6050, &hi2c1, MPU6050_Addr))
		{
			PRINTF("MPU6050 initialize failed %d times.\r\n", ++i);
			osDelay(100);
		}
		osDelay(100);

		MPU6050_Get_Accelerometer(&MPU6050);

		g = MPU6050.AccX * MPU6050.AccX + MPU6050.AccY * MPU6050.AccY + MPU6050.AccZ * MPU6050.AccZ;

		if(((int)g)==0)
			res = 1;
		else
			res = 0;

		PRINTF("MPU6050 initialize failed %d times.\r\n", ++i);
	}


	PRINTF("MPU6050 initialized successfully in %d times!\r\n", ++i);

	/* Init encoder */
	KDL_Encoder_Init(&Encoder[0], &htim1);
	KDL_Encoder_Init(&Encoder[1], &htim2);
	KDL_Encoder_Init(&Encoder[2], &htim3);
	KDL_Encoder_Init(&Encoder[3], &htim8);
	/* Start encoders */
	KDL_Encoder_Start(&Encoder[0]);
	KDL_Encoder_Start(&Encoder[1]);
	KDL_Encoder_Start(&Encoder[2]);
	KDL_Encoder_Start(&Encoder[3]);
	/* Test encoder */
	for(;;)
	{
		/* Fetch encoder */
		for(i=0;i<4;i++)
		{
			KDL_Encoder_Convert(&Encoder[i]);\
			printf("Velocity %d is %ld.\r\n", i, Encoder[i].Velocity);
		}
		/* Fetch mpu6050 */
		MPU6050_Get_Temperature(&MPU6050);
		osDelay(1);
		MPU6050_Get_Gyroscope(&MPU6050);
		osDelay(1);
		MPU6050_Get_Accelerometer(&MPU6050);
		osDelay(1);

		PRINTF("[MPU6050] Temperature %d;\r\n", (int)(MPU6050.Temp*10));
		PRINTF("[MPU6050] GyrX %d;\r\n", (int)(MPU6050.GyrX*10));
		PRINTF("[MPU6050] GyrY %d;\r\n", (int)(MPU6050.GyrY*10));
		PRINTF("[MPU6050] GyrZ %d;\r\n", (int)(MPU6050.GyrZ*10));
		PRINTF("[MPU6050] AccX %d;\r\n", (int)(MPU6050.AccX*100));
		PRINTF("[MPU6050] AccY %d;\r\n", (int)(MPU6050.AccY*100));
		PRINTF("[MPU6050] AccZ %d;\r\n", (int)(MPU6050.AccZ*100));

		PRINTF("[EXTI] %lu times.\r\n", exti_times);

		HCSR04_Measure(&HCSR04);
		PRINTF("[HCSR04] %lu.\r\n", HCSR04.Ticks);

		osDelay(1000);
	}
	/* Exit thread */
	osThreadExit();
}


void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	exti_times++;
	return;
}
